#include <mutex>
#include <thread>

#include <lcm/lcm-cpp.hpp>
#include "Request.hpp"
#include "Response.hpp"


#include <iostream>
#include "stdio.h"
#include <onnxruntime_cxx_api.h>

class RL_Xdog {
public:

    int init_onnx();

    void handleMessage(const lcm::ReceiveBuffer *rbuf, const std::string &chan,
                       const  my_lcm::Request *request);//获取机器人反馈

    float Kp = 20.0;
    float Kd = 0.5;
    float infer_dt = 0.02;//50Hz

    float cmd_x = 1.0;
    float cmd_y = 0.0;
    float cmd_z = 0.0;

    //关节初始角度
    float init_pos[12] = {0.0,-1.0,2.0,  0.0,-1.0,2.0,  0.0,-1.0,2.0,  0.0,-1.0,2.0};

    //缩放因子
    float omega_scale=  0.25;
    float dof_pos_scale =   1.0;
    float dof_vel_scale =   0.05;

    float cmd_lin_vel = 2.0;
    float cmd_ang_vel = 0.25;

    float action_scale[12] = {0.25,0.25,0.25, 0.25,0.25,0.25, 0.25,0.25,0.25, 0.25,0.25,0.25};


    std::vector<float> obs;


    int input_nodes_num;
    int output_nodes_num;
    std::vector<std::string> input_node_names;
    std::vector<std::string> output_node_names;
    std::array<const char*, 1> input_name;
    std::array<const char*, 1> output_name;
    Ort::Session *onnx_session = nullptr;
 
    std::string policy_path;
 
    std::vector<double> action;

private:


};

